/*
 * map_saver
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <ORGANIZATION> nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <cstdio>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/GetMap.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"

using namespace std;

/**
 * @brief Map generation node.
 */
class MapGenerator
{

public:
    MapGenerator(const std::string &mapname, int threshold_occupied, int threshold_free)
            : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied),
              threshold_free_(threshold_free)
    {
        ros::NodeHandle n;
        ROS_INFO("Waiting for the map");
        map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
    }

    ~MapGenerator() {map_sub_.shutdown();}

    void mapCallback(const nav_msgs::OccupancyGridConstPtr &map)
    {
        ROS_INFO("Received a %d X %d map @ %.3f m/pix",
                 map->info.width, map->info.height, map->info.resolution);
        std::string mapdatafile = mapname_ + ".pgm";
        ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
        FILE *out = fopen(mapdatafile.c_str(), "w");
        if (!out)
        {
            ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
            return;
        }
        fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
                map->info.resolution, map->info.width, map->info.height);
        for (unsigned int y = 0; y < map->info.height; y++)
        {
            for (unsigned int x = 0; x < map->info.width; x++)
            {
                unsigned int i = x + (map->info.height - y - 1) * map->info.width;
                // [0,free)
                if (map->data[i] >= 0 && map->data[i] <= threshold_free_) fputc(254, out);
                    // (occ,255]
                else if (map->data[i] >= threshold_occupied_) fputc(000, out);
                    //occ [0.25,0.65]
                else fputc(205, out);
            }
        }
        fclose(out);
        std::string mapmetadatafile = mapname_ + ".yaml";
        ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
        FILE *yaml = fopen(mapmetadatafile.c_str(), "w");
        /*
          resolution: 0.100000
          origin: [0.000000, 0.000000, 0.000000]
          negate: 0
          occupied_thresh: 0.65
          free_thresh: 0.196
         */
        geometry_msgs::Quaternion orientation = map->info.origin.orientation;
        tf2::Matrix3x3 mat(tf2::Quaternion(
                orientation.x,
                orientation.y,
                orientation.z,
                orientation.w
        ));
        double yaw, pitch, roll;
        mat.getEulerYPR(yaw, pitch, roll);
        fprintf(yaml,
                "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
                mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y,
                yaw);
        fclose(yaml);
        ROS_INFO("Done\n");
        saved_map_ = true;
    }
    std::string mapname_;
    ros::Subscriber map_sub_;
    bool saved_map_;
    int threshold_occupied_;
    int threshold_free_;
};

#define USAGE "Usage: \n" \
              "  map_saver -h\n"\
              "  map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_saver");
    std::string mapname = "map";
    int threshold_occupied = 65;
    int threshold_free = 25;
    for (int i = 1; i < argc; i++)
    {
        if (!strcmp(argv[i], "-h"))
        {
            puts(USAGE);
            return 0;
        }
        else if (!strcmp(argv[i], "-f"))
        {
            if (++i < argc) mapname = argv[i];
            else
            {
                puts(USAGE);
                return 1;
            }
        }
        else if (!strcmp(argv[i], "--occ"))
        {
            if (++i < argc)
            {
                threshold_occupied = std::atoi(argv[i]);
                if (threshold_occupied < 1 || threshold_occupied > 100)
                {
                    ROS_ERROR("threshold_occupied must be between 1 and 100");
                    return 1;
                }

            }
            else
            {
                puts(USAGE);
                return 1;
            }
        }
        else if (!strcmp(argv[i], "--free"))
        {
            if (++i < argc)
            {
                threshold_free = std::atoi(argv[i]);
                if (threshold_free < 0 || threshold_free > 100)
                {
                    ROS_ERROR("threshold_free must be between 0 and 100");
                    return 1;
                }
            }
            else
            {
                puts(USAGE);
                return 1;
            }
        }
        else
        {
            puts(USAGE);
            return 1;
        }
    }
    if (threshold_occupied <= threshold_free)
    {
        ROS_ERROR("threshold_free must be smaller than threshold_occupied");
        return 1;
    }
    MapGenerator mg(mapname, threshold_occupied, threshold_free);
    while (!mg.saved_map_ && ros::ok()) ros::spinOnce();
    return 0;
}


